#include <stdio.h>
#include <unistd.h>
#include "ohos_init.h"
#include "cmsis_os2.h"

#define STACK_SIZE   2048
#define MQUEUE_SIZE  3

typedef struct {
    osThreadId_t tid;
    int count;
} MsgData;

osMessageQueueId_t g_mqId;

void TaskSend(int *arg) {
    static int count = 0;
    MsgData data;

    while (1) {
        data.tid = osThreadGetId();
        data.count = count;

        printf("[Send] %s send %d to message queue.\r\n", osThreadGetName(osThreadGetId()), count);

        osMessageQueuePut(g_mqId, (const int *)&data, 0, osWaitForever);

        count++;
        osDelay( 5 );
    }
}

void TaskRecv(int *arg) {
    MsgData data;

    while (1) {
        osMessageQueueGet(g_mqId, (int *)&data, NULL, osWaitForever);

        printf("[%s] Get %d msg from %s \r\n",
            osThreadGetName(osThreadGetId()), data.count, osThreadGetName(data.tid));

        osDelay( 5 );
    }
}

osThreadId_t TaskNew(char *name, osThreadFunc_t func, int *arg) {
    osThreadAttr_t attr = {
        name, 0, NULL, 0, NULL, 1024*2, osPriorityNormal, 0, 0
    };

    osThreadId_t tid = osThreadNew(func, arg, &attr);
    if (tid == NULL) {
        printf("[Main] %s Create Failed\r\n", name);
    } 
    else {
        printf("[Main] %s Create OK\r\n", name);
    }

    return tid;
}

void MQueueDemoMain(int *arg) {
    (int)arg;

    g_mqId = osMessageQueueNew(MQUEUE_SIZE, sizeof(MsgData), NULL);

    osThreadId_t ctid1 = TaskNew("Recv1", TaskRecv, NULL);
    osThreadId_t ctid2 = TaskNew("Recv2", TaskRecv, NULL);
    osThreadId_t ptid1 = TaskNew("Send1", TaskSend, NULL);
    osThreadId_t ptid2 = TaskNew("Send2", TaskSend, NULL);
    osThreadId_t ptid3 = TaskNew("Send3", TaskSend, NULL);

    osDelay( 20 );
    uint32_t cap = osMessageQueueGetCapacity( g_mqId );
    printf("[Main] Queue capacity: %u\r\n", cap);

    uint32_t msg_size =  osMessageQueueGetMsgSize( g_mqId );
    printf("[Main] Queue Msg size: %u\r\n", msg_size);

    uint32_t count = osMessageQueueGetCount( g_mqId );
    printf("[Main] Queue count: %u\r\n", count);

    uint32_t space = osMessageQueueGetSpace( g_mqId );
    printf("[Main] Queue space: %u.\r\n", space);

    osThreadTerminate( ctid1 );
    osThreadTerminate( ctid2 );
    osThreadTerminate( ptid1 );
    osThreadTerminate( ptid2 );
    osThreadTerminate( ptid3 );

    osMessageQueueDelete( g_mqId );

    printf("[Main] Exit\r\n");
}

SYS_RUN( MQueueDemoMain );
